/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2021-12-06     MBW       the first version
 */
#include "msh_set.h"
void parameter_set1(int argc, char**argv)
{
    if (!rt_strcmp(argv[1], "pitch_angel"))
    {
        rt_kprintf("**********%s**********\n",argv[1]);
        rt_thread_mdelay(20);
        PID_PAR.angel_kp.x = atof(argv[2]);
        PID_PAR.angel_ki.x = atof(argv[3]);
        PID_PAR.angel_kd.x = atof(argv[4]);
        printf("...reset %s PID :kp %.2f , ki %.2f , kd %.2f \r\n",argv[1]
                ,PID_PAR.angel_kp.x, PID_PAR.angel_ki.x, PID_PAR.angel_kd.x);
    }
    else if (!rt_strcmp(argv[1], "pitch_rate"))
    {
        rt_kprintf("**********%s**********\n",argv[1]);
        rt_thread_mdelay(20);
        PID_PAR.rate_kp.x = atof(argv[2]);
        PID_PAR.rate_ki.x = atof(argv[3]);
        PID_PAR.rate_kd.x = atof(argv[4]);
        printf("...reset %s PID :kp %.2f , ki %.2f , kd %.2f \r\n",argv[1]
                ,PID_PAR.rate_kp.x, PID_PAR.rate_ki.x, PID_PAR.rate_kd.x);
    }
    else if (!rt_strcmp(argv[1], "roll_angel"))
    {
        rt_kprintf("**********%s**********\n",argv[1]);
        rt_thread_mdelay(20);
        PID_PAR.angel_kp.y = atof(argv[2]);
        PID_PAR.angel_ki.y = atof(argv[3]);
        PID_PAR.angel_kd.y = atof(argv[4]);
        printf("...reset %s PID :kp %.2f , ki %.2f , kd %.2f \r\n",argv[1]
                ,PID_PAR.angel_kp.y, PID_PAR.angel_ki.y, PID_PAR.angel_kd.y);
    }
    else if (!rt_strcmp(argv[1], "roll_rate"))
    {
        rt_kprintf("**********%s**********\n",argv[1]);
        rt_thread_mdelay(20);
        PID_PAR.rate_kp.y = atof(argv[2]);
        PID_PAR.rate_ki.y = atof(argv[3]);
        PID_PAR.rate_kd.y = atof(argv[4]);
        printf("...reset %s PID :kp %.2f , ki %.2f , kd %.2f \r\n",argv[1]
                ,PID_PAR.rate_kp.y, PID_PAR.rate_ki.y, PID_PAR.rate_kd.y);
    }
    else if (!rt_strcmp(argv[1], "yaw_angel"))
    {
        rt_kprintf("**********%s**********\n",argv[1]);
        rt_thread_mdelay(20);
        PID_PAR.angel_kp.z = atof(argv[2]);
        PID_PAR.angel_ki.z = atof(argv[3]);
        PID_PAR.angel_kd.z = atof(argv[4]);
        printf("...reset %s PID :kp %.2f , ki %.2f , kd %.2f \r\n",argv[1]
                ,PID_PAR.angel_kp.z, PID_PAR.angel_ki.z, PID_PAR.angel_kd.z);
    }
    else if (!rt_strcmp(argv[1], "yaw_rate"))
    {
        rt_kprintf("**********%s**********\n",argv[1]);
        rt_thread_mdelay(20);
        PID_PAR.rate_kp.z = atof(argv[2]);
        PID_PAR.rate_ki.z = atof(argv[3]);
        PID_PAR.rate_kd.z = atof(argv[4]);
        printf("...reset %s PID :kp %.2f , ki %.2f , kd %.2f \r\n",argv[1]
                ,PID_PAR.rate_kp.z, PID_PAR.rate_ki.z, PID_PAR.rate_kd.z);
    }
    else if (!rt_strcmp(argv[1], "pos_x"))
    {
        rt_kprintf("**********%s**********\n",argv[1]);
        rt_thread_mdelay(20);
        PID_PAR.dis_kp.x = atof(argv[2]);
        PID_PAR.dis_ki.x = atof(argv[3]);
        PID_PAR.dis_kd.x = atof(argv[4]);
        printf("...reset %s PID :kp %.2f , ki %.2f , kd %.2f \r\n",argv[1]
                ,PID_PAR.dis_kp.x, PID_PAR.dis_ki.x, PID_PAR.dis_kd.x);
    }
    else if (!rt_strcmp(argv[1], "vel_x"))
    {
        rt_kprintf("**********%s**********\n",argv[1]);
        rt_thread_mdelay(20);
        PID_PAR.vel_kp.x = atof(argv[2]);
        PID_PAR.vel_ki.x = atof(argv[3]);
        PID_PAR.vel_kd.x = atof(argv[4]);
        printf("...reset %s PID :kp %.2f , ki %.2f , kd %.2f \r\n",argv[1]
                ,PID_PAR.vel_kp.x, PID_PAR.vel_ki.x, PID_PAR.vel_kd.x);
    }
    else if (!rt_strcmp(argv[1], "pos_y"))
    {
        rt_kprintf("**********%s**********\n",argv[1]);
        rt_thread_mdelay(20);
        PID_PAR.dis_kp.y = atof(argv[2]);
        PID_PAR.dis_ki.y = atof(argv[3]);
        PID_PAR.dis_kd.y = atof(argv[4]);
        printf("...reset %s PID :kp %.2f , ki %.2f , kd %.2f \r\n",argv[1]
                ,PID_PAR.dis_kp.y, PID_PAR.dis_ki.y, PID_PAR.dis_kd.y);
    }
    else if (!rt_strcmp(argv[1], "vel_y"))
    {
        rt_kprintf("**********%s**********\n",argv[1]);
        rt_thread_mdelay(20);
        PID_PAR.vel_kp.y = atof(argv[2]);
        PID_PAR.vel_ki.y = atof(argv[3]);
        PID_PAR.vel_kd.y = atof(argv[4]);
        printf("...reset %s PID :kp %.2f , ki %.2f , kd %.2f \r\n",argv[1]
                ,PID_PAR.vel_kp.y, PID_PAR.vel_ki.y, PID_PAR.vel_kd.y);
    }
    else if (!rt_strcmp(argv[1], "pos_z"))
    {
        rt_kprintf("**********%s**********\n",argv[1]);
         rt_thread_mdelay(20);
         PID_PAR.dis_kp.z = atof(argv[2]);
         PID_PAR.dis_ki.z = atof(argv[3]);
         PID_PAR.dis_kd.z = atof(argv[4]);
         printf("...reset %s PID :kp %.2f , ki %.2f , kd %.2f \r\n",argv[1]
                 ,PID_PAR.dis_kp.z, PID_PAR.dis_ki.z, PID_PAR.dis_kd.z);
    }
    else if (!rt_strcmp(argv[1], "vel_z"))
    {
        rt_kprintf("**********%s**********\n",argv[1]);
        rt_thread_mdelay(20);
        PID_PAR.vel_kp.z = atof(argv[2]);
        PID_PAR.vel_ki.z = atof(argv[3]);
        PID_PAR.vel_kd.z = atof(argv[4]);
        printf("...reset %s PID :kp %.2f , ki %.2f , kd %.2f \r\n",argv[1]
                ,PID_PAR.vel_kp.z, PID_PAR.vel_ki.z, PID_PAR.vel_kd.z);
    }
}
MSH_CMD_EXPORT(parameter_set1,parameter_set1);
